﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Brain.Bepu;
using BEPUphysics;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Input;
using BEPUphysics.MathExtensions;
using BEPUphysics.BroadPhaseEntries;
using BEPUphysics.CollisionRuleManagement;

namespace Brain.General
{
    public class DefaultGamerControllerInputManager : IGamerControllerInputManager
    {
        public IGamerController Parent { get; protected set; }

        /// <summary>
        /// Physics representation of this character
        /// </summary>
        public CharacterController Controller;

        protected bool Initialized;

        public PlayerInfo PlayerInfo;

        public DefaultGamerControllerInputManager(PlayerInfo player)
        {
            this.PlayerInfo = player;

            rayCastFilter = RayCastFilter;
        }

        public virtual void Initialize(IGamerController parent)
        {
            this.Parent = parent;

            Controller = new CharacterController(Vector3.Zero, 
                parent.CharacterInfo.Height, 
                parent.CharacterInfo.CrouchingHeight, 
                parent.CharacterInfo.Radius,
                parent.CharacterInfo.Mass);

            if (Engine.Instance.Physics3DManager is BepuPhysicsManager)
            {
                BepuPhysicsManager physics = Engine.Instance.Physics3DManager as BepuPhysicsManager;
                physics.Space.Add(Controller);
            }
            else
            {
                throw new Exception("Brain 2 General Library needs 'BepuPhysicsManager' to work!");
            }

            Initialized = true;
        }

        public bool IsCrouching = false;
        public bool PlayerWantsToCrouch = false;

        public virtual GamerControllerState GetState()
        {
            if (!Initialized)
            {
                throw new Exception("Cannot call GetState() without initalizing the IGamerControllerInputManager!");
            }

            GamerControllerState state = new GamerControllerState();

            float dt = (float)Engine.Instance.GameTime.ElapsedGameTime.TotalSeconds;
            CharacterInfo character = this.Parent.CharacterInfo;
            GamerControllerOptions options = Parent.Options;

            if (Parent.Camera != null)
            {
                if (Parent.Options.MouseRotatesCamera)
                {
                    Vector2 mouseDif = Engine.Instance.InputManager.GetMouseDifference();
                    Parent.Camera.RotationRadians += new Vector3(-MathHelper.ToRadians(mouseDif.Y), -MathHelper.ToRadians(mouseDif.X), 0) * options.Sensitivity;
                }
                Vector3 yOnly = new Vector3(0, Parent.Camera.RotationRadians.Y, 0);
                state.RotationMatrix = MatrixUtil.CreateRotationMatrix(yOnly);
                Controller.Body.Orientation = QuaternionUtil.CreateQuaternion(yOnly);
                Parent.Camera.RotationMatrix = MatrixUtil.CreateRotationMatrix(Parent.Camera.RotationRadians);

                if (Parent.Options.CameraType == CameraType.FirstPerson)
                {
                    //Note that the character controller's update method is not called here; this is because it is handled within its owning space.
                    //This method's job is simply to tell the character to move around based on the Camera and input.
                    if (Parent.Options.UseCameraSmoothing)
                    {
                        //First, find where the camera is expected to be based on the last position and the current velocity.
                        //Note: if the character were a free-floating 6DOF character, this would need to include an angular velocity contribution.
                        //And of course, the camera orientation would be based on the character's orientation.
                        Parent.Camera.Position = Parent.Camera.Position + Controller.Body.LinearVelocity * dt;
                        //Now compute where it should be according the physical body of the character.
                        Vector3 up = Controller.Body.OrientationMatrix.Up;
                        Vector3 bodyPosition = Controller.Body.Position;
                        Vector3 goalPosition = bodyPosition + up * (this.IsCrouching ? character.StandingCameraOffset : character.CrouchingCameraOffset);

                        //Usually, the camera position and the goal will be very close, if not matching completely.
                        //However, if the character steps or has its position otherwise modified, then they will not match.
                        //In this case, we need to correct the camera position.

                        //To do this, first note that we can't correct infinite errors.  We need to define a bounding region that is relative to the character
                        //in which the camera can interpolate around.  The most common discontinuous motions are those of upstepping and downstepping.
                        //In downstepping, the character can teleport up to the character's MaximumStepHeight downwards.
                        //In upstepping, the character can teleport up to the character's MaximumStepHeight upwards, and the body's CollisionMargin horizontally.
                        //Picking those as bounds creates a constraining cylinder.

                        Vector3 error = goalPosition - Parent.Camera.Position;
                        float verticalError = Vector3.Dot(error, up);
                        Vector3 horizontalError = error - verticalError * up;
                        //Clamp the vertical component of the camera position within the bounding cylinder.
                        if (verticalError > Controller.StepManager.MaximumStepHeight)
                        {
                            Parent.Camera.Position -= up * (Controller.StepManager.MaximumStepHeight - verticalError);
                            verticalError = Controller.StepManager.MaximumStepHeight;
                        }
                        else if (verticalError < -Controller.StepManager.MaximumStepHeight)
                        {
                            Parent.Camera.Position -= up * (-Controller.StepManager.MaximumStepHeight - verticalError);
                            verticalError = -Controller.StepManager.MaximumStepHeight;
                        }
                        //Clamp the horizontal distance too.
                        float horizontalErrorLength = horizontalError.LengthSquared();
                        float margin = Controller.Body.CollisionInformation.Shape.CollisionMargin;
                        if (horizontalErrorLength > margin * margin)
                        {
                            Vector3 previousHorizontalError = horizontalError;
                            Vector3.Multiply(ref horizontalError, margin / (float)Math.Sqrt(horizontalErrorLength), out horizontalError);
                            Parent.Camera.Position -= horizontalError - previousHorizontalError;
                            horizontalErrorLength = margin * margin;
                        }
                        //Now that the error/camera position is known to lie within the constraining cylinder, we can perform a smooth correction.

                        //This removes a portion of the error each frame.
                        //Note that this is not framerate independent.  If fixed time step is not enabled,
                        //a different smoothing method should be applied to the final error values.
                        //float errorCorrectionFactor = .3f;

                        //This version is framerate independent, although it is more expensive.
                        float errorCorrectionFactor = (float)(1 - Math.Pow(.000000001, dt));
                        Parent.Camera.Position += up * (verticalError * errorCorrectionFactor);
                        Parent.Camera.Position += horizontalError * errorCorrectionFactor;

                        Parent.Camera.Up = up;
                        Parent.Camera.Target = Parent.Camera.RotationMatrix.Forward + Parent.Camera.Position;
                    }
                    else
                    {
                        Vector3 up = Controller.Body.OrientationMatrix.Up;

                        Parent.Camera.Position = Controller.Body.Position + (this.IsCrouching ? character.StandingCameraOffset : character.CrouchingCameraOffset) * up;
                        Parent.Camera.Up = up;
                        Parent.Camera.Target = Parent.Camera.RotationMatrix.Forward + Parent.Camera.Position;
                    }
                }
                else if (options.CameraType == CameraType.ThirdPerson)
                {
                    Vector3 offset;
                    if (options.TransformOffset)
                    {
                        offset = Matrix3X3.Transform(options.OffsetFromTarget, Controller.Body.BufferedStates.InterpolatedStates.OrientationMatrix);
                    }
                    else
                    {
                        offset = options.OffsetFromTarget;
                    }
                    Vector3 lookAt = Controller.Body.BufferedStates.InterpolatedStates.Position + offset;
                    Vector3 backwards = Parent.Camera.RotationMatrix.Backward;

                    //Find the earliest ray hit that isn't the chase target to position the camera appropriately.
                    RayCastResult result;
                    if (Controller.Body.Space.RayCast(new Ray(lookAt, backwards), options.DistanceFromPlayer, rayCastFilter, out result))
                    {
                        Parent.Camera.Position = lookAt + (result.HitData.T) * backwards; //Put the camera just before any hit spot.
                    }
                    else
                    {
                        Parent.Camera.Position = lookAt + (options.DistanceFromPlayer) * backwards;
                    }
                    Parent.Camera.Target = lookAt;
                    Parent.Camera.Up = Parent.Camera.RotationMatrix.Up;
                }
            }

#if XBOX || WINDOWS
            WindowsOptions winOptions = options.WindowsKeys;
            Xbox360Options xboxOptions = options.XboxKeys;

            Vector2 totalMovement = Vector2.Zero;

            Vector2 movementThumbstick = Vector2.Zero;
            Vector2 lookThumbstick = Vector2.Zero;
            switch (xboxOptions.MovementThumb)
            {
                case Thumbsticks.Left:
                    movementThumbstick = Engine.Instance.InputManager.GetLeftThumbstick(PlayerInfo.PlayerIndex);
                    lookThumbstick = Engine.Instance.InputManager.GetRightThumbstick(PlayerInfo.PlayerIndex);
                    break;
                case Thumbsticks.Right:
                    movementThumbstick = Engine.Instance.InputManager.GetRightThumbstick(PlayerInfo.PlayerIndex);
                    lookThumbstick = Engine.Instance.InputManager.GetLeftThumbstick(PlayerInfo.PlayerIndex);
                    break;
            }

            Vector3 forward = Controller.Body.OrientationMatrix.Forward;
            forward.Y = 0;
            forward.Normalize();
            Vector3 right = Controller.Body.OrientationMatrix.Right;
            totalMovement += movementThumbstick.Y * new Vector2(forward.X, forward.Z);
            totalMovement += movementThumbstick.X * new Vector2(right.X, right.Z);
            Controller.HorizontalMotionConstraint.MovementDirection = Vector2.Normalize(totalMovement);

            Controller.StanceManager.DesiredStance = Engine.Instance.InputManager.IsButtonDown(xboxOptions.Crouch, PlayerInfo.PlayerIndex) ? Stance.Crouching : Stance.Standing;

            //Jumping
            if (Engine.Instance.InputManager.IsButtonPress(xboxOptions.Jump, PlayerInfo.PlayerIndex))
            {
                Controller.Jump();
            }

            if (Parent.Options.CanCrouch)
            {
                if (Parent.Options.CrouchHold)
                {
                    if (Engine.Instance.InputManager.IsKeyDown(winOptions.Crouch) ||
                        Engine.Instance.InputManager.IsButtonDown(xboxOptions.Crouch, PlayerInfo.PlayerIndex))
                    {
                        Controller.StanceManager.DesiredStance = Stance.Crouching;
                    }
                    else
                    {
                        Controller.StanceManager.DesiredStance = Stance.Standing;
                    }
                }
                else
                {
                    if (Engine.Instance.InputManager.IsKeyPress(winOptions.Crouch) ||
                        Engine.Instance.InputManager.IsButtonPress(xboxOptions.Crouch, PlayerInfo.PlayerIndex))
                    {
                        PlayerWantsToCrouch = !PlayerWantsToCrouch;
                    }
                }

                if (PlayerWantsToCrouch)
                {
                    Controller.StanceManager.DesiredStance = Stance.Crouching;
                }
                else
                {
                    Controller.StanceManager.DesiredStance = Stance.Standing;
                }
            }

            //Collect the movement impulses.
            Vector3 movementDir;

            if (Engine.Instance.InputManager.IsKeyDown(winOptions.MovementForward))
            {
                movementDir = Parent.Camera.RotationMatrix.Forward;
                totalMovement += Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
            }
            if (Engine.Instance.InputManager.IsKeyDown(winOptions.MovementBackward))
            {
                movementDir = Parent.Camera.RotationMatrix.Forward;
                totalMovement -= Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
            }
            if (Engine.Instance.InputManager.IsKeyDown(winOptions.MovementLeft))
            {
                movementDir = Parent.Camera.RotationMatrix.Left;
                totalMovement += Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
            }
            if (Engine.Instance.InputManager.IsKeyDown(winOptions.MovementRight))
            {
                movementDir = Parent.Camera.RotationMatrix.Right;
                totalMovement += Vector2.Normalize(new Vector2(movementDir.X, movementDir.Z));
            }

            if (totalMovement == Vector2.Zero)
            {
                Controller.HorizontalMotionConstraint.MovementDirection = Vector2.Zero;
            }
            else
            {
                Controller.HorizontalMotionConstraint.MovementDirection = Vector2.Normalize(totalMovement);
            }

            //Jumping
            if (Engine.Instance.InputManager.IsKeyPress(winOptions.Jump))
            {
                Controller.Jump();
            }

            Xbox360GamePadState gamePadState = new Xbox360GamePadState();
            gamePadState.LookThumstick = lookThumbstick;
            gamePadState.MovementThumstick = movementThumbstick;
            Parent.GamePadState = gamePadState;

#endif
            state.Position = Controller.Body.Position;

            return state;
        }

        Func<BroadPhaseEntry, bool> rayCastFilter;
        bool RayCastFilter(BroadPhaseEntry entry)
        {
            return entry != Controller.Body.CollisionInformation && (entry.CollisionRules.Personal <= CollisionRule.Normal);
        }
    }
}
